{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 计算走测车的坐标变换公式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "expression of absolute wind in ground coordinate:\n",
      ">>>Uxs:\t-Uy*(sin(pitch)*sin(roll)*sin(theta) + cos(roll)*cos(theta)) + Uz*(sin(pitch)*sin(theta)*cos(roll) - sin(roll)*cos(theta)) - (-Ux + speed)*sin(theta)*cos(pitch)\n",
      ">>>Uys:\t-Uy*(sin(pitch)*sin(roll)*cos(theta) - sin(theta)*cos(roll)) + Uz*(sin(pitch)*cos(roll)*cos(theta) + sin(roll)*sin(theta)) - (-Ux + speed)*cos(pitch)*cos(theta)\n",
      ">>>Uzs:\t-Uy*sin(roll)*cos(pitch) + Uz*cos(pitch)*cos(roll) + (-Ux + speed)*sin(pitch)\n",
      "jscode is:\n",
      ">>> Uxs = -Uy*(Math.sin(pitch)*Math.sin(roll)*Math.sin(theta) + Math.cos(roll)*Math.cos(theta)) + Uz*(Math.sin(pitch)*Math.sin(theta)*Math.cos(roll) - Math.sin(roll)*Math.cos(theta)) + (Ux - speed)*Math.sin(theta)*Math.cos(pitch);\n",
      ">>> Uys = -Uy*(Math.sin(pitch)*Math.sin(roll)*Math.cos(theta) - Math.sin(theta)*Math.cos(roll)) + Uz*(Math.sin(pitch)*Math.cos(roll)*Math.cos(theta) + Math.sin(roll)*Math.sin(theta)) + (Ux - speed)*Math.cos(pitch)*Math.cos(theta);\n",
      ">>> Uzs = -Uy*Math.sin(roll)*Math.cos(pitch) + Uz*Math.cos(pitch)*Math.cos(roll) + (-Ux + speed)*Math.sin(pitch);\n"
     ]
    }
   ],
   "source": [
    "from sympy.vector import CoordSys3D,express\n",
    "from sympy import symbols,pi\n",
    "from sympy import jscode,simplify\n",
    "from math import atan2,sqrt\n",
    "import numpy as np\n",
    "ground_coord = CoordSys3D('ground')# 西-南-天 地面坐标系\n",
    "\n",
    "# 车体坐标系为 前-左-上\n",
    "theta = symbols('theta') # 航向 正北为0正东为90度，绕-z轴转\n",
    "pitch = symbols('pitch') # 上坡偏为正, 绕-y轴转\n",
    "roll= symbols('roll') #向右偏为正, 绕+x轴旋转\n",
    "Ux,Uy,Uz,speed = symbols('Ux,Uy,Uz,speed')\n",
    "\n",
    "# 通过欧拉角旋转，从西-南-天 地面坐标系 得到 车体的前-左-上坐标系\n",
    "# 旋转顺序依次为：\n",
    "# -z, -y, +x \n",
    "# 最开始 前-左-上 和 西-南-天是对应的\n",
    "# 首先 车体绕-z轴逆时针旋转theta+pi/2角\n",
    "# 然后 车体绕-y轴逆时针旋转pitch角\n",
    "# 最后 车体绕+x轴逆时针旋转roll角\n",
    "car_coord = ground_coord.orient_new_body('car', -(pi/2+theta), -pitch, roll, '321')\n",
    "\n",
    "# 相对风\n",
    "rel_wind_car_coord = -Ux*car_coord.i - Uy*car_coord.j + Uz*car_coord.k\n",
    "speed_car_coord = speed *car_coord.i\n",
    "abs_wind_car_coord = rel_wind_car_coord + speed_car_coord\n",
    "\n",
    "# 把绝对风矢量按地面坐标系表示。\n",
    "ex = express(abs_wind_car_coord, ground_coord)\n",
    "print(f'expression of absolute wind in ground coordinate:')\n",
    "for n,(k,v) in zip('Uxs,Uys,Uzs'.split(','),ex.components.items()):\n",
    "    print(f'>>>{n}:\\t{v}')\n",
    "\n",
    "# 转化为javascript代码\n",
    "print('jscode is:')\n",
    "for n,(k,v) in zip('Uxs,Uys,Uzs'.split(','),ex.components.items()):\n",
    "    print('>>>',jscode(simplify(v),assign_to=n) ) \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle (- Uy \\left(\\sin{\\left(pitch \\right)} \\sin{\\left(roll \\right)} \\sin{\\left(\\theta \\right)} + \\cos{\\left(roll \\right)} \\cos{\\left(\\theta \\right)}\\right) + Uz \\left(\\sin{\\left(pitch \\right)} \\sin{\\left(\\theta \\right)} \\cos{\\left(roll \\right)} - \\sin{\\left(roll \\right)} \\cos{\\left(\\theta \\right)}\\right) - \\left(- Ux + speed\\right) \\sin{\\left(\\theta \\right)} \\cos{\\left(pitch \\right)})\\mathbf{\\hat{i}_{ground}} + (- Uy \\left(\\sin{\\left(pitch \\right)} \\sin{\\left(roll \\right)} \\cos{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\cos{\\left(roll \\right)}\\right) + Uz \\left(\\sin{\\left(pitch \\right)} \\cos{\\left(roll \\right)} \\cos{\\left(\\theta \\right)} + \\sin{\\left(roll \\right)} \\sin{\\left(\\theta \\right)}\\right) - \\left(- Ux + speed\\right) \\cos{\\left(pitch \\right)} \\cos{\\left(\\theta \\right)})\\mathbf{\\hat{j}_{ground}} + (- Uy \\sin{\\left(roll \\right)} \\cos{\\left(pitch \\right)} + Uz \\cos{\\left(pitch \\right)} \\cos{\\left(roll \\right)} + \\left(- Ux + speed\\right) \\sin{\\left(pitch \\right)})\\mathbf{\\hat{k}_{ground}}$"
      ],
      "text/plain": [
       "(-Uy*(sin(pitch)*sin(roll)*sin(theta) + cos(roll)*cos(theta)) + Uz*(sin(pitch)*sin(theta)*cos(roll) - sin(roll)*cos(theta)) - (-Ux + speed)*sin(theta)*cos(pitch))*ground.i + (-Uy*(sin(pitch)*sin(roll)*cos(theta) - sin(theta)*cos(roll)) + Uz*(sin(pitch)*cos(roll)*cos(theta) + sin(roll)*sin(theta)) - (-Ux + speed)*cos(pitch)*cos(theta))*ground.j + (-Uy*sin(roll)*cos(pitch) + Uz*cos(pitch)*cos(roll) + (-Ux + speed)*sin(pitch))*ground.k"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ex"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "1.7320508075688772"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 风速\n",
    "WS = lambda Uxs,Uys,Uzs:sqrt(Uxs**2+Uys**2+Uzs**2)\n",
    "WS(1,1,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(90.0, 180.0, 270.0, 0.0)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 风向\n",
    "WD = lambda Uxs,Uys:(atan2(Uxs, Uys)/np.pi*180 + 360)%360\n",
    "\n",
    "# 风矢量是在西-南-天地面坐标系定义的\n",
    "\n",
    "WD(1,0),WD(0,-1),WD(-1,0),WD(0,1) # 东风90°, 南风180°，西风270°，北风0°,"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 计算YYR UAV的坐标变换公式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ">>>python code is:\n",
      "wX = uz*(sin(pitch)*sin(roll)*sin(yaw) - cos(roll)*cos(yaw)) + (uy + vx)*sin(yaw)*cos(pitch) + (uz + vz)*(sin(pitch)*sin(yaw)*cos(roll) + sin(roll)*cos(yaw)) \n",
      "wY = uz*(sin(pitch)*sin(roll)*cos(yaw) + sin(yaw)*cos(roll)) + (uy + vx)*cos(pitch)*cos(yaw) + (uz + vz)*(sin(pitch)*cos(roll)*cos(yaw) - sin(roll)*sin(yaw)) \n",
      "wZ = uz*sin(roll)*cos(pitch) - (uy + vx)*sin(pitch) + (uz + vz)*cos(pitch)*cos(roll) \n",
      "wS = sqrt((uz*(sin(pitch)*sin(roll)*sin(yaw) - cos(roll)*cos(yaw)) + (uy + vx)*sin(yaw)*cos(pitch) + (uz + vz)*(sin(pitch)*sin(yaw)*cos(roll) + sin(roll)*cos(yaw)))**2 + (uz*(sin(pitch)*sin(roll)*cos(yaw) + sin(yaw)*cos(roll)) + (uy + vx)*cos(pitch)*cos(yaw) + (uz + vz)*(sin(pitch)*cos(roll)*cos(yaw) - sin(roll)*sin(yaw)))**2) \n",
      "wD = 180*(atan2(-uz*(sin(pitch)*sin(roll)*sin(yaw) - cos(roll)*cos(yaw)) - (uy + vx)*sin(yaw)*cos(pitch) - (uz + vz)*(sin(pitch)*sin(yaw)*cos(roll) + sin(roll)*cos(yaw)), -uz*(sin(pitch)*sin(roll)*cos(yaw) + sin(yaw)*cos(roll)) - (uy + vx)*cos(pitch)*cos(yaw) - (uz + vz)*(sin(pitch)*cos(roll)*cos(yaw) - sin(roll)*sin(yaw)))) % (2*pi)/pi \n"
     ]
    }
   ],
   "source": [
    "from sympy.vector import CoordSys3D,express\n",
    "from sympy import symbols,pi\n",
    "from sympy import simplify,pycode\n",
    "from sympy import atan2,sqrt\n",
    "import sympy as syp\n",
    "from sympy.utilities.lambdify import lambdify\n",
    "import numpy as np\n",
    "\n",
    "ux,uy,uz,vx,vy,vz = symbols('ux,uy,uz,vx,vy,vz')\n",
    "# 车体坐标系为 前-左-上\n",
    "yaw = symbols('yaw') # 假设偏航角==航向角，正北为0正东为90度，绕Zb轴转\n",
    "pitch = symbols('pitch') # 抬头为正，绕Yb轴旋转\n",
    "roll= symbols('roll') #向右偏为正，绕Xb轴旋转\n",
    "\n",
    "ground_coord = CoordSys3D('ground')# 东-北-天 地面坐标系\n",
    "\n",
    "# 通过欧拉角旋转，从东-北-天 地面坐标系 得到 UAV机体的前-右-下坐标系\n",
    "# 旋转顺序依次为：\n",
    "# Zb, Yb, Xb \n",
    "# 最开始 东-北-天（Xg-Yg-Zg)和右-前-上(Yb-Xb-(-Zb)) 是对应的\n",
    "uav_coord = ground_coord.orient_new_body('uav',pi/2-yaw, pitch, roll, 'ZYX')\n",
    "\n",
    "# 相对风\n",
    "rel_wind_uav_coord = uy*uav_coord.i + uz*uav_coord.j + uz*uav_coord.k\n",
    "\n",
    "# 无人机地速，\n",
    "vy = 0 # 假设vy等于0\n",
    "ground_speed_uav_coord = vx *uav_coord.i + vy*uav_coord.j + vz*uav_coord.k\n",
    "\n",
    "abs_wind_uav_coord = rel_wind_uav_coord + ground_speed_uav_coord\n",
    "\n",
    "# 把绝对风矢量按地面坐标系表示。\n",
    "ex = express(abs_wind_uav_coord, ground_coord)\n",
    "\n",
    "# 风速\n",
    "wX = ex.components[ground_coord.i]\n",
    "wY = ex.components[ground_coord.j]\n",
    "wZ = ex.components[ground_coord.k]\n",
    "\n",
    "wS = sqrt(wX**2+wY**2)\n",
    "# 风向，\n",
    "wD = syp.Mod(atan2(-wX,-wY),2*pi)*180/pi\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# 转化为python代码\n",
    "print('>>>python code is:')\n",
    "for n,v in zip('wX,wY,wZ,wS,wD'.split(','),[wX,wY,wZ,wS,wD]):\n",
    "    print(f'{n} = {pycode(expr = v, fully_qualified_modules = False)} ')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ">>>test input\n",
      "ux:0\n",
      "uy:0\n",
      "uz:0\n",
      "vx:0\n",
      "vz:0\n",
      "pitch:0\n",
      "roll:0\n",
      "yaw:0\n",
      ">>>test output\n",
      "wX = 0\n",
      "wY = 0\n",
      "wZ = 0\n",
      "wS = 0\n",
      "wD = nan\n"
     ]
    }
   ],
   "source": [
    "test_input={\n",
    "    ux:0,\n",
    "    uy:0,\n",
    "    uz:0, \n",
    "    vx:0,\n",
    "    vz:0,\n",
    "    pitch:0,\n",
    "    roll: 0,\n",
    "    yaw:0\n",
    "}\n",
    "\n",
    "print('>>>test input')\n",
    "for k,v in test_input.items():\n",
    "    print(f'{k}:{v}')\n",
    "print('>>>test output')\n",
    "for n,v in zip('wX,wY,wZ,wS,wD'.split(','),[wX,wY,wZ,wS,wD]):\n",
    "    print(f'{n} = {v.subs(test_input)}')\n",
    "\n",
    "# sympy.atan2(0,0) == NaN，math.atan2(0,0) == 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ">>>test input\n",
      "ux:1\n",
      "uy:1\n",
      "uz:1\n",
      "vx:0\n",
      "vz:-1\n",
      "pitch:pi/180\n",
      "roll:pi/180\n",
      "yaw:pi/2\n",
      ">>>test output\n",
      "wX = 1.00015228164684\n",
      "wY = 0.999847695156391\n",
      "wZ = -0.00000265808603302700\n",
      "wS = 1.41421356237060\n",
      "wD = 225.008725760234\n"
     ]
    }
   ],
   "source": [
    "test_input={\n",
    "    ux:1,\n",
    "    uy:1,\n",
    "    uz:1, \n",
    "    vx:0,\n",
    "    vz:-1,\n",
    "    pitch:pi/180*1,\n",
    "    roll: pi/180*1,\n",
    "    yaw:pi/2\n",
    "}\n",
    "\n",
    "print('>>>test input')\n",
    "for k,v in test_input.items():\n",
    "    print(f'{k}:{v}')\n",
    "print('>>>test output')\n",
    "for n,v in zip('wX,wY,wZ,wS,wD'.split(','),[wX,wY,wZ,wS,wD]):\n",
    "    print(f'{n} = {syp.N(v.subs(test_input))}')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-3.599687899082569, 0.2055408212460914, 8.0]"
      ]
     },
     "execution_count": 74,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from sympy.utilities.lambdify import lambdify\n",
    "f = lambdify([ux,uy,uz,vx,vz,pitch,roll,yaw],[wX,wY,wZ])\n",
    "f(2,1,3,1,5,0,0,100)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
